#include "vehicle_lateral3dis.h"

namespace auto_ros
{
namespace control
{
VehicleLateral3dis::VehicleLateral3dis(double mass, double cf, double cr, double lf, double lr, double iz,
									   double max_lateral_acc, double max_tire_f_angle)
	: ContinuousLinearSys(4, 1, 3, 1),
	  mass_(mass), cf_(cf), cr_(cr), lf_(lf),
	  lr_(lr), iz_(iz), max_lateral_acc_(max_lateral_acc),
	  max_tire_f_angle_(max_tire_f_angle)

{
	speed_threshold_ = 1.0;
	c_yaw_rate_ = -lf_ / 1.0 * cf_ + lr_ / 1.0 * cr_;
	c_beta_ = -(cf_ + cr_);
	c_delta_ = cf_;

	d_yaw_rate_ = -lf_ * lf_ / 1.0 * cf_ - lr_ * lr_ / 1.0 * cr_;
	d_delta_ = lf_ * cf_;
	d_beta_ = -lf_ * cf_ + lr_ * cr_;
	speed_threshold_ = 1.0;
	//init speed is 1.0m/s
	state_matrix_ << c_beta_ / mass_ / 1.0, c_yaw_rate_ / mass_ / 1.0 - 1, 0, 0,
		d_beta_ / iz_, d_yaw_rate_ / iz_, 0, 0,
		0, 0, 0, 1.0,
		c_beta_ / mass_ / 1.0, c_yaw_rate_ / mass_ / 1.0, 0, 0;
	control_matrix_ << c_delta_ / mass_ / 1.0,
		d_delta_ / iz_,
		0,
		c_delta_ / mass_ / 1.0;
	distur_matrix_ << 0, 1 / 1.0, 0,
		0, 0, 1,
		0, 0, 0,
		-1.0, 1 / 1.0, 0;
	out_state_matrix_ << 0, 0, 1.0, 0;
	out_distur_matrix_ << 0, 0, 0;
}
void VehicleLateral3dis::speed_update_matrix(double speed)
{
	if (std::abs(speed) < speed_threshold_)
	{
		if (speed >= 0)
			speed = speed_threshold_;
		else
			speed = -speed_threshold_;
	}
	speed_ = speed;
	c_yaw_rate_ = -lf_ / speed * cf_ + lr_ / speed * cr_;
	d_yaw_rate_ = -lf_ * lf_ / speed * cf_ - lr_ * lr_ / speed * cr_;
	state_matrix_ << c_beta_ / mass_ / speed, c_yaw_rate_ / mass_ / speed - 1, 0, 0,
		d_beta_ / iz_, d_yaw_rate_ / iz_, 0, 0,
		0, 0, 0, speed,
		c_beta_ / mass_ / speed, c_yaw_rate_ / mass_ / speed, 0, 0;
	control_matrix_ << c_delta_ / mass_ / speed,
		d_delta_ / iz_,
		0,
		c_delta_ / mass_ / speed;
	distur_matrix_(0, 1) = 1.0 / speed;
	distur_matrix_(3, 0) = -speed;
	distur_matrix_(3, 1) = 1.0 / speed;
}

double VehicleLateral3dis::mass()
{
	return mass_;
}
double VehicleLateral3dis::lf()
{
	return lf_;
}
double VehicleLateral3dis::lr()
{
	return lr_;
}
double VehicleLateral3dis::cf()
{
	return cf_;
}
double VehicleLateral3dis::cr()
{
	return cr_;
}
double VehicleLateral3dis::iz()
{
	return iz_;
}
double VehicleLateral3dis::max_tire_f_angle()
{
	return max_tire_f_angle_;
}
double VehicleLateral3dis::speed_threshold()
{
	return speed_threshold_;
}

} // namespace control
} // namespace auto_ros